#include "Constraint.h"

void CLengthConstraint::init(CPhysicsSegment segment, Scalar fLength)
{
    
    m_Segment = segment;
    
    //Default to the distance the entities begin apart if no length is supplied
    if (fLength == -1)
        m_fLength = segment.getLength();
    else
        m_fLength = fLength;
    
}

void CSpringConstraint::update()
{
    
    Vector vBetween = m_Segment.getVector();
    Scalar fLength = vBetween.getLength();
    
    //Normalise the between vector
    vBetween /= fLength;
    
    //Find the difference between its actual length and its desired length
    Scalar fDeltaLength = m_fLength - fLength;
    
    //Find the relative velocity of the end points of the segment in the direction of the segment
    Scalar fVel = (m_Segment.getPoint2()->getVel() - m_Segment.getPoint1()->getVel()) % vBetween;
    
    m_Segment.getPoint1()->applyForce(vBetween * (-m_fSpringConstant * fDeltaLength + m_fDampingConstant * fVel));
    m_Segment.getPoint2()->applyForce(vBetween * (m_fSpringConstant * fDeltaLength - m_fDampingConstant * fVel));
    
}

void CSlideConstraint::init(CPhysicsPoint* pSlidingPoint, CSegment segment)
{
    
    m_pSlidingPoint = pSlidingPoint;
    m_Segment = segment;
    
}

void CSlideConstraint::update()
{
    
    Vector vSegment = m_Segment.getVector();
    
    //Solve for parameter t along line segment where the sliding point is nearest
    Scalar t = (vSegment % (m_pSlidingPoint->getPos() - m_Segment.getStart())) / vSegment.getLength2();
    
    //Constrain to the ends of the segment
    if (t < 0.f)
        t = 0.f;
    else if (t > 1.f)
        t = 1.f;
    
    //Shift the point onto the line segment
    m_pSlidingPoint->setPos(m_Segment.getStart() + t * vSegment);
    
}
